#ifndef STEPPER_H
#define STEPPER_H 

#include <Arduino.h>
#include "../logger.h"
#include "../globals.hpp"
#include "filesystem.h"
// use 12 bit precission for LEDC timer
#define LEDC_TIMER_12_BIT 12
// use 5000 Hz as a LEDC base frequency
#define LEDC_BASE_FREQ 5000
#define MAX_FREQ = 100000 // 例如100kHz
#define DIR0 22
#define PUL0 21
#define ENA0 27
#define DIR1 3
#define PUL1 1
#define MOTOR_CHANNEL_0 0
#define MOTOR_CHANNEL_1 2

const char *motor0_setting_path = "/motor.json";
const char *motor1_setting_path = "/motor1.json";

class Stepper { 
    public:
        Stepper(uint8_t, uint8_t, uint8_t, uint8_t);
        void run_freq(int);
        bool run_rps(float);
        void enable();
        void disable();
        void set_param(uint16_t, float);
        int rps_to_pulse_freq(float );
        int mmps_to_pulse_freq(float );
        float pulse_freq_to_rps(int);
        uint16_t microstepping = 16;
        float step_angle = 1.8; // 每步角度
        float pulses_per_rev = 0.; // 每脉冲圈数
        float pitch = 1.0; // 螺距
        bool is_param_set = false;
        uint8_t motor_channel;
    
    private:
        uint8_t dir_pin;
        uint8_t pul_pin;
        uint8_t enable_pin;
        bool use_with_uart0 = false;  // pul和uart公用
        bool dir = LOW; // 方向
        

};

Stepper :: Stepper(uint8_t pul_pin, uint8_t dir_pin, uint8_t enable_pin, uint8_t motor_channel){
    // 初始化一个电机，提供其关联管脚与电机号
    this->dir_pin = dir_pin;
    this->pul_pin = pul_pin;
    this->enable_pin = enable_pin; // 设置启用管脚
    this->motor_channel = motor_channel;
    pinMode(dir_pin, OUTPUT);
    // digitalWrite(dir_pin, LOW);
    pinMode(enable_pin, OUTPUT);
    if (this->pul_pin==1) {
        this -> use_with_uart0 = true;
    }
    digitalWrite(enable_pin, LOW); // 默认禁用电机

    ledcSetup(motor_channel, LEDC_BASE_FREQ, LEDC_TIMER_12_BIT);
    // ledcAttachPin(pul_pin, motor_channel);

    logf("[STEPPER %d] Initialized", motor_channel);
}

void Stepper :: run_freq(int freq_speed){
    
    if (freq_speed == 0) {
        // ledcWriteTone(motor_channel, 0); // 停止输出
        ledcDetachPin(this->pul_pin); // 断开引脚
        digitalWrite(this->pul_pin,  HIGH); // 停止输出
        if (this -> use_with_uart0){
            Serial.begin(115200);
            Serial.setTimeout(0);
        }
        return;
    }else if (freq_speed > 0) {
        this -> enable(); // 确保电机已启用
        this->dir = HIGH;
        if (this -> use_with_uart0){
            Serial.flush();
            Serial.end();
        }
        ledcAttachPin(this->pul_pin, this->motor_channel);
        digitalWrite(this->dir_pin, HIGH);
        ledcWriteTone(this->motor_channel, freq_speed); 
    }else{
        this -> enable(); // 确保电机已启用
        this->dir = LOW;
        if (this -> use_with_uart0){
            Serial.flush();
            Serial.end();
        }
        ledcAttachPin(this->pul_pin, this->motor_channel);
        digitalWrite(this->dir_pin, LOW);
        ledcWriteTone(this->motor_channel, -freq_speed);
    }
}

// 电机上电
void Stepper :: enable() {
    digitalWrite(this->enable_pin, LOW);
    logf("[STEPPER %d] Enabled", this->motor_channel);
}

// 电机下电
void Stepper :: disable() {
    digitalWrite(this->enable_pin, HIGH);
    logf("[STEPPER %d] Disabled", this->motor_channel);
}

void Stepper :: set_param(uint16_t microstepping, float step_angle) {
    // 设置电机参数
    this->microstepping = microstepping;
    this->step_angle = step_angle;
    this->is_param_set = true;
    this->pulses_per_rev = (this->step_angle / (float)this->microstepping) / 360.0f;
    logf("[STEPPER %d] Param set: microstepping=%d, step_angle=%.2f", 
         this->motor_channel, this->microstepping, this->step_angle);
    logf("[STEPPER %d] pulses_per_rev calced into %.8f",
        this->motor_channel, pulses_per_rev);
}

// 把转每分转换为脉冲频率，根据步距角和微步数计算
int Stepper ::  rps_to_pulse_freq(float rps) {
    static float freq = 0.;
    if (rps == 0) return 0;
    if (rps < 0) {rps = -rps;}
    freq = rps / pulses_per_rev;
    logf("[STEPPER %d] rps %.2f r/s convert to %d Hz",
        this->motor_channel, rps, (int)freq);
    return (int)freq;
}

// 把转每分转换为脉冲频率，根据步距角和微步数计算
float Stepper ::  pulse_freq_to_rps(int freq) {
    static float rps = 0.;
    if (freq == 0) return 0;
    if (freq < 0) {freq = -freq;}
    rps = (float)freq * pulses_per_rev;
    logf("[STEPPER %d] freq %d Hz convert to %.2f r/s",
        this->motor_channel, freq, rps);
    return rps;
}

// 把毫米每秒转换为脉冲频率，根据步距角,微步和螺距计算
int Stepper ::  mmps_to_pulse_freq(float mmps) {

    static float freq = 0.;
    static float rps = 0.;
    if (mmps == 0) return 0;
    if (mmps < 0) {mmps = -mmps;}
    rps = mmps / this->pitch;
    
    return rps_to_pulse_freq(rps);
}

// 以转每秒为单位运行电机
bool Stepper :: run_rps(float speed) {
    if (!this->is_param_set){
        logf("[STEPPER] Motor %d param haven't set, ignore.", this->motor_channel);
        return false;
    }else{
        if (speed == 0.) {
            this->run_freq(0);
            return true;
        }else if (speed > 0.){
            this->run_freq(rps_to_pulse_freq(speed));
        }else{
            this->run_freq(-rps_to_pulse_freq(speed));
        }
        return true;
    }
}

// 初始化电机配置
void loadMotorConfig(Stepper &stepper, const char *motor_setting_path)
{   
    logf("[STEPPER %d] Reading motor config from \"%s\"", stepper.motor_channel, motor_setting_path);
    if (!xtouch_filesystem_exist(UMEKO_FS, motor_setting_path)){
        logf("[STEPPER %d] No such file: \"%s\"", stepper.motor_channel, motor_setting_path);
        return;
    }
    fs::File file = xtouch_filesystem_open(UMEKO_FS, motor_setting_path);

    StaticJsonDocument<512> doc;

    // Deserialize the JSON document
    DeserializationError error = deserializeJson(doc, file);
    if (error){
        logf("[STEPPER %d] Failed to read motor config", stepper.motor_channel);
        xtouch_filesystem_deleteFile(XTOUCH_FS, motor_setting_path);
        return;
    }

    stepper.pitch = doc["pitch"].as<float>();
    stepper.set_param(
        doc["microstepping"].as<uint16_t>(),
        doc["step_angle"].as<float>());
    file.close();
    stepper.is_param_set = true;
    logf("[STEPPER %d] Loaded \"%s\": ms=%d, pitch=%.2f, sa=%.2f", 
                stepper.motor_channel,
                motor_setting_path,
                stepper.microstepping,
                stepper.pitch, 
                stepper.step_angle);
}

void saveMotorConfig(Stepper &stepper, const char *motor_setting_path)
{
    StaticJsonDocument<512> doc;
    doc["microstepping"] = stepper.microstepping;
    doc["pitch"] = stepper.pitch;
    doc["step_angle"] = stepper.step_angle;
    xtouch_filesystem_writeJson(XTOUCH_FS, motor_setting_path, doc);
}

// 验证微步设置的合法性
bool check_ms(uint16_t ms){
    return (ms == 1 || ms == 2 || ms == 4 || ms == 8 || ms == 16 || ms == 32 || ms == 64 || ms == 128 || ms == 256);
}
bool check_sa(float sa){
    return (sa == 1.8 || sa == 0.9);
}

// 步进电机0: PUL=21, DIR=22, ENA=27, CHANNEL=0
Stepper stepper_x(PUL0, DIR0, ENA0, MOTOR_CHANNEL_0);
// 步进电机0: PUL=17, DIR=16, ENA=27, CHANNEL=2
Stepper stepper_y(PUL1, DIR1, ENA0, MOTOR_CHANNEL_1);

void config_setting(Stepper stepper, String subcmd, const char * path){
    if(subcmd.startsWith("ms ")){
        uint16_t ms = subcmd.substring(3).toInt();
        if (check_ms(ms)){
            stepper.set_param(ms, stepper.step_angle);
            logf("[STEPPER %d] Set microstepping to %d", stepper.motor_channel, ms);
            saveMotorConfig(stepper, path);
        }else{
            logf("[STEPPER %d] Invalid microstepping value", stepper.motor_channel);
        }
    }else if(subcmd.startsWith("sa ")) {
        float step_angle = subcmd.substring(3).toFloat();
        if (check_sa(step_angle)){
            stepper.set_param(stepper.microstepping, step_angle);
            logf("[STEPPER %d] Set step angle to %.2f", stepper.motor_channel, step_angle);
            saveMotorConfig(stepper, path);
        }else{
            logf("[STEPPER %d] Invalid step angle value", stepper.motor_channel);
        }
    }else if(subcmd.startsWith("save")){
        saveMotorConfig(stepper, path);
        logf("[STEPPER %d] Motor config saved to %s", stepper.motor_channel, path);
    }
}

void stepper_dealer(String cmd) {
    static int   freq_speed = 0;
    static float rps = 0;
    // Stepper 0 CLI 控制命令
    if(cmd.startsWith("f0 ")){
        freq_speed = cmd.substring(3).toInt();
        stepper_x.run_freq(freq_speed);
        logf("[STEPPER 0] Set to %d Hz", freq_speed);
    }else if (cmd.startsWith("rps0 ")){
        rps = cmd.substring(5).toFloat();
        if (stepper_x.run_rps(rps)){ logf("[STEPPER 0] Set to %.3f rad/s", rps); }
    }
    // Stepper 1 CLI 控制命令
    else if(cmd.startsWith("f1 ")){
        freq_speed = cmd.substring(3).toInt();
        stepper_y.run_freq(freq_speed);
        logf("[STEPPER 1] Set to %d Hz", freq_speed);
    }else if (cmd.startsWith("rps1 ")){
        rps = cmd.substring(5).toFloat();
        if (stepper_y.run_rps(rps)){ logf("[STEPPER 1] Set to %.3f rad/s", rps); }
    }
    // 步进参数设置命令
    else if(cmd.startsWith("set0 ")){
        config_setting(stepper_x, cmd.substring(5), motor0_setting_path);
    }else if(cmd.startsWith("set1 ")){
        config_setting(stepper_y, cmd.substring(5), motor1_setting_path);
    }
    // 未知命令
    else{
        logf("[CLI] Unknown command \"%s\"", cmd.c_str());
    }
}

// 从文件系统中初始化步进电机设置
void motor_init_params(){
    loadMotorConfig(stepper_x, motor0_setting_path);
    loadMotorConfig(stepper_y, motor1_setting_path);
    // stepper_x.enable(); // 确保电机已启用
}

void motor_disable(){
    stepper_x.disable(); // 确保电机已启用
    stepper_y.disable(); // 确保电机已启用
}
void motor_enable(){
    stepper_x.enable(); // 确保电机已启用
    stepper_y.enable(); // 确保电机已启用
}

#endif